#ifndef OMPL_GEOMETRIC_PLANNERS_PRM_HSPRM_H
#define OMPL_GEOMETRIC_PLANNERS_PRM_HSPRM_H

#include <ompl/base/Planner.h>
#include <ompl/datastructures/NearestNeighbors.h>
#include <ompl/geometric/planners/PlannerIncludes.h>
#include <utility>
#include <vector>
#include <map>
#include <queue>

#include "hplanner_ompl/collision_estimator.h"


namespace ompl
{

namespace geometric
{

class hsPRM : public base::Planner
{
public:

  typedef typename base::ApproxCSpaceOracleMultiprobe CSpaceOracle;
  typedef typename base::CollisionStatusLineEstimator CollisionStatusEstimator;


  hsPRM(const base::SpaceInformationPtr& si);

  virtual ~hsPRM();

  virtual void setProblemDefinition(const base::ProblemDefinitionPtr& pdef);

  void setMaxNearestNeighbors(unsigned int maxNearestNeighbors);

  unsigned int getMaxNearestNeighbors() const;

  virtual void getPlannerData(base::PlannerData& data) const;


  template<template<typename T> class NN>
  void setNearestNeighbors()
  {
    nn_.reset(new NN<Milestone*>());
  }

  virtual void deepClear();

  virtual void clear();

  virtual void setup();

  virtual bool solve(const base::PlannerTerminationCondition& ptc);

  void reconstructLastSolution();

protected:

  class Milestone
  {
  public:

    Milestone() : state(NULL), index(0)
    {
    }

    /** \brief Automatically allocate memory for a milestone's state */
    Milestone(const base::SpaceInformationPtr& si) : state(si->allocState()), index(0)
    {
    }

    ~Milestone()
    {
    }

    /** \brief The state corresponding to the milestone */
    base::State *state;

    /** \brief The index of this milestone in the array of milestones (BasicPRM::milestones_) */
    unsigned int index;

    /** \brief The id of the connected component this milestone is part of */
    unsigned long component;

    /** \brief The array of milestones that can be connected to with valid paths */
    std::vector<Milestone*> adjacent;

    /** \brief The cost of the edges indicated by \e adjacent */
    std::vector<double> costs;
  };

  void freeMemory();

  bool haveSolution(const std::vector<Milestone*>& start, const std::vector<Milestone*>& goal, std::pair<Milestone*, Milestone*>* endpoints = NULL);

  void constructSolution(const Milestone* start, const Milestone* goal);

  double distanceFunction(const Milestone* a, const Milestone* b) const;

  bool checkMotion(const base::State* s1, const base::State* s2, bool initgoal) const;

  Milestone* addMilestoneOnly(base::State *state);

  Milestone* addMilestone(base::State *state);

  void uniteComponents(Milestone *m1, Milestone *m2);

  void nearestNeighbors(Milestone* milestone, std::vector<Milestone*>& nbh);

  void updateConnectivity();

  void growRoadmap(const std::vector<Milestone*>& start,
                         const std::vector<Milestone*>& goal,
                         const base::PlannerTerminationCondition& ptc,
                         base::State* workState);

  base::StateSamplerPtr sampler_;

  boost::shared_ptr<NearestNeighbors<Milestone*> > nn_;

  std::vector<Milestone*> milestones_;

  std::vector<Milestone*> startM_;

  std::vector<Milestone*> goalM_;

  const Milestone* lastStart_;

  const Milestone* lastGoal_;

  unsigned int maxNearestNeighbors_;

  std::map<unsigned long, unsigned long> componentSizes_;

  unsigned long componentCount_;

  unsigned long nLastMilestones_;

  RNG rng_;

  mutable unsigned int nPointCollisions_;

  mutable unsigned int nPathCollisions_;

  mutable unsigned int nCollisions_;

  CSpaceOracle* cspaceOracle_;
  CollisionStatusEstimator* collisionEstimator_;


};

}

}

#endif
